/*
   Here we will describe functions for
   process of interferometer image.
*/
#include "interferometer.h"


LINES     new_Lines= {0,300,0,0,0,0};
LINES     LineParameters= {0,300,0,0,0,0};
int32_t   send_counter = 0;

/* This function just show interferention image on the screen. */
void ordinal_screen(void)
{
    uint32_t count;
    uint32_t r=0,g=0,b=0;
    uint16_t CMOS_Data;

    if( Vsync == 2 )
    {
        LCD_SetCursor(0, 319);
        LCD_WriteRAM_Prepare(); /* Prepare to write GRAM */
        FIFO_RRST_L();
        FIFO_RCLK_L();

        FIFO_RCLK_H();
        FIFO_RRST_H();
        FIFO_RCLK_L();

        FIFO_RCLK_H();

        for( count = 0; count < 76800; count++ )
        {
            FIFO_RCLK_L();

            CMOS_Data = (GPIOC->IDR<<8) & 0xff00;	  /* ( GPIO_ReadInputData(GPIOC) << 8 ) & 0xff00; */
            FIFO_RCLK_H();

            FIFO_RCLK_L();
            CMOS_Data |= (GPIOC->IDR) & 0x00ff;	  /* ( GPIO_ReadInputData(GPIOC) ) & 0x00ff; */
            FIFO_RCLK_H();

            b = ( CMOS_Data>>0 )  & 0x1f;
            g = ( CMOS_Data>>5 )  & 0x3f;
            r = ( CMOS_Data>>11 ) & 0x1f;
            CMOS_Data =  (r<<11) + (g<<5) + (b<<0);

            LCD_WriteRAM(CMOS_Data);
       }
       Vsync = 0;
    }
}



/* This function do calculation of interferention image. */
void screen_processing(void )
{
    uint8_t text_buf[20];
    uint32_t count_x=0, count_y = 319;
    uint32_t bw_flag=0,edge_counter = 0,rise_edge_counter = 0;
    EDGE edge_flag = FALL;
    LINE_EDGE new_Edge[10] = {FALL,0};
    LINE_EDGE RiseEdge[10] = {FALL,0};

    uint32_t r = 0,g = 0,b = 0,temp;
    uint32_t bright_tmp = 0;
    static uint32_t bright = 0;
    uint16_t CMOS_Data;


    if( Vsync == 2 )
    {
        FIFO_RRST_L();
        FIFO_RCLK_L();
        FIFO_RCLK_H();
        FIFO_RRST_H();
        FIFO_RCLK_L();
        FIFO_RCLK_H();
        bright_tmp=0;

        for( count_x = 0; count_x < 140; count_x++ )
        {
            for( count_y = 0; count_y < 320; count_y++ )
            {
                FIFO_RCLK_L();
                FIFO_RCLK_H();
                FIFO_RCLK_L();
                FIFO_RCLK_H();
            }
        }
        for( count_x = 141; count_x < 180; count_x++ )
        {
            for( count_y = 0; count_y < 320; count_y++ )
            {
                FIFO_RCLK_L();
                CMOS_Data = (GPIOC->IDR<<8) & 0xff00;	  /* ( GPIO_ReadInputData(GPIOC) << 8 ) & 0xff00; */
                FIFO_RCLK_H();

                FIFO_RCLK_L();
                CMOS_Data |= (GPIOC->IDR) & 0x00ff;	  /* ( GPIO_ReadInputData(GPIOC) ) & 0x00ff; */
                FIFO_RCLK_H();

                b = ( CMOS_Data>>0 )  & 0x1f;
                g = ( CMOS_Data>>5 )  & 0x3f;
                r = ( CMOS_Data>>11 ) & 0x1f;

                /* Black & White */
#if 1
                temp = (b+r+(g>>1))/3;
                bright_tmp=bright_tmp+temp;

                if(temp >= bright)
                {
                    b = 0x1F;
                    g = 0x3F;
                    r = 0x1F;
                }
                else
                {
                    b = 0;
                    g = 0;
                    r = 0;
                }
#endif
                CMOS_Data =  (r<<11) + (g<<5) + (b<<0);
                if((count_x >= 147) && (count_x <= 154) )
                {
                    /* motion detector. */
                    if(count_x == 150)
                    {
                        if(count_y==0)
                        { /* Set color of line for zero possition. */
                            bw_flag = CMOS_Data;
                        }
                        if(CMOS_Data != bw_flag)//We found black line
                        {
                            if(bw_flag == 0)
                            {
                                edge_flag = RISE; /* Edge from black to white.*/
                            }
                            else
                            {
                                edge_flag = FALL; /* Edge from white to black.*/
                            }
                            bw_flag = CMOS_Data;

                            /* Save edge to array. */
                            /* Here we are saving RISE edges only. */
                            if(rise_edge_counter < 10)
                            {
                                if(edge_flag == RISE)
                                {
                                    if(rise_edge_counter == 0)
                                    {
                                        RiseEdge[rise_edge_counter].Edge = RISE;
                                        RiseEdge[rise_edge_counter].Position = count_y;
                                        rise_edge_counter++;
                                    }
                                    else
                                    {
                                        if( (count_y - RiseEdge[rise_edge_counter-1].Position)> 5 )
                                        {
                                            RiseEdge[rise_edge_counter].Edge = RISE;
                                            RiseEdge[rise_edge_counter].Position = count_y;
                                            rise_edge_counter++;
                                        }
                                    }
                                }
                            }

                            /* Here we are saving all edges.*/
                            if(count_y <= 318)
                            {
                                if( edge_counter < 10)/* If array is not full*/
                                {/* save current edge to array.*/
                                    if(edge_counter == 0)
                                    {
                                        new_Edge[edge_counter].Position = count_y;
                                        new_Edge[edge_counter].Edge = edge_flag;
                                        edge_counter++;
                                    }
                                    else
                                    {
                                        /* We are filtering here line position noise.
                                        *  We suppose that RISE->FALL or FALL->RISE
                                        *  distance should be more than 10 screen dots
                                        *  TODO: add magic value to definition.
                                        */
                                        if( (count_y - new_Edge[edge_counter-1].Position)> 10 )
                                        {
                                            new_Edge[edge_counter].Position = count_y;
                                            new_Edge[edge_counter].Edge = edge_flag;
                                            edge_counter++;
                                        }
                                    }
                                }/* <10 */
                            }
                        }/* CMOS_Data != bw_flag */
                    }/* count_x == 150 */

                    if(count_x == 149)
                    {
                        CMOS_Data = 0x1F<<11;
                    }
                    LCD_SetCursor(count_x, 319-count_y);
                    LCD_WriteRAM_Prepare(); /* Prepare to write GRAM */
                    LCD_WriteRAM(CMOS_Data);
                }/* (count_x >= 147) && (count_x <= 154) */
            }/* for( count_y = 0; count_y < 320; count_y++ ) */
        }/* for( count_x = 141; count_x < 180; count_x++ ) */

        /* calculation.*/
        line_parameters_calculation(RiseEdge, rise_edge_counter);
        center_of_line_parameters_calculation(new_Edge, edge_counter);

        /*
        *  Here we can dynamicaly change brightness.
        *  But now we are using fixed value.
        */
        bright = 12;/* bright_tmp/76800; */
        Vsync = 0;
    }/*         if( Vsync == 2 ) */

    /* Print position to the screen. */
    sprintf((char*)text_buf,"                ");
    GUI_Text_Rotated(220, 50, text_buf,BG_SYSTEM_TEXT_COLOR, BG_SYSTEM_SCREEN_COLOR,3);
    sprintf((char*)text_buf,"RedY=%d",new_Lines.LineShift);
    GUI_Text_Rotated(220, 10, text_buf,BG_SYSTEM_TEXT_COLOR, BG_SYSTEM_SCREEN_COLOR,3);
}


/* Send data to PC using my standart protocol. */
void send_interferometer_data( INTERFEROMETER_DATA_TYPE data_type, uint32_t data)
{
    uint8_t send_buffer[SEND_PACKET_BUFFER_SIZE];
    uint8_t i = 0;
    /*
        We have send small packet.
        Lets form it manualy.
    */
    send_buffer[0] = 0x73;                         /* HEAD of packet. */
    send_buffer[1] = 0x05  ;                       /* SIZE of packet. */
    send_buffer[2] = data_type;                    /* Name of variable of packet. */
    send_buffer[3] = (uint8_t)(data       & 0xFF); /* LSB of u32 data. */
    send_buffer[4] = (uint8_t)((data>>8)  & 0xFF); /* 1 of u32 data. */
    send_buffer[5] = (uint8_t)((data>>16) & 0xFF); /* 2 of u32 data. */
    send_buffer[6] = (uint8_t)((data>>24) & 0xFF); /* MSB of u32 data. */
    send_buffer[7] =  0xFF - (uint8_t)(send_buffer[2]+send_buffer[3]+send_buffer[4]+send_buffer[5]+send_buffer[6]); /* CRC of packet. */

    for(i = 0; i < SEND_PACKET_BUFFER_SIZE; i++)
    {
        USART_SendData(USART1, send_buffer[i]);
        vTaskDelay( 1 );/* If we will not call vTaskDelay we will loose most of transmitted bytes. */
    }
}




/* 1 is error , 0 is ok */
/*TODO: add correct error detection. */
/* This function calculate centers of white lines and looking shift value.*/
uint32_t center_of_line_parameters_calculation(LINE_EDGE *EdgeArray, uint32_t EdgeCounter)
{
    uint32_t result = 0;
    int32_t center_of_lines[MAXIMUM_LINES_AMOUNT] = {0};
    static int32_t old_center_of_lines[5] = {0};
    static int32_t old_distance_to_zero_point = 0;
    static int32_t old_closest_index = 0;
    int32_t amount_of_lines = 0;
    int32_t line_shift = 0;
    int32_t closest_index = 0;
    int32_t distance_to_zero_point = 0;
    int32_t tmp = 0;
    int32_t i = 0;

    i = 0;
    amount_of_lines = 0;
    /* Calculation of array of center of lines. */
    while(i < EdgeCounter)
    {
        /* Start from RISE edge, start of white line. */
        if(EdgeArray[i].Edge == RISE)
        {
            i++;
           /*
            *  Check if next element of array is exist.
            *  End of array can be RISE edge too and we can not use it
            *  for calculation of center of white line.
            */
            if(i < EdgeCounter)
            {
                if(amount_of_lines < MAXIMUM_LINES_AMOUNT)/* MAXIMUM_LINES_AMOUNT is amount of center of line array. */
                {
                    /*
                    * Looking for center of white lines help us to remove brightness error.
                    * (Sensor automaticaly change sensetivity and it change line width.)
                    */
                    center_of_lines[amount_of_lines] = EdgeArray[i-1].Position + (EdgeArray[i].Position - EdgeArray[i-1].Position)/2;
                    amount_of_lines++;
                }
            }
        }
        i++;/* "else" is not neccesary here. */
    }

    /* Looking closest line to zero point. */
    distance_to_zero_point = 10000; /* exactly out of screen value. */
    for( i = 0; i < amount_of_lines; i++ )
    {
        tmp = ZERO_POSSITION_POINT - center_of_lines[i];
        if(abs(distance_to_zero_point) > abs(tmp))
        {
            closest_index = i;
            distance_to_zero_point = tmp;
        }
        else if(abs(distance_to_zero_point) == abs(tmp))
        {
            /* If distance is same we will use oldest index. */
            if(old_closest_index == i)
            {
                closest_index = i;
            }
        }
    }

    /* Calculaton of value of line shift.*/
    line_shift =  center_of_lines[closest_index] - old_center_of_lines[old_closest_index];
    /* If  shift more than half period it means that we should change line index to newest one. */
    if(abs(line_shift) > abs((center_of_lines[0] - center_of_lines[1])/2))
    {
         line_shift =  center_of_lines[closest_index] - old_center_of_lines[closest_index];
    }

    /* Save current centers position. */
    old_closest_index = closest_index;
    old_distance_to_zero_point = distance_to_zero_point;
    for( i = 0; i < amount_of_lines; i++ )
    {
       old_center_of_lines[i] = center_of_lines[i];
    }



    new_Lines.LineShift = line_shift;
    new_Lines.AbsolutPosition = new_Lines.AbsolutPosition + new_Lines.LineShift;//line_shift;
    /* It will used for testing purpers. */
    new_Lines.CenterPosition = center_of_lines[1];
    /* Send data. */
    /* To decrease data stream.
    *  TODO: Change magic value.
    */
    if(send_counter >= 2)
    {
        send_interferometer_data( LINE_POSSITION, new_Lines.AbsolutPosition);
        send_interferometer_data( LINE_WIDTH, new_Lines.CenterPosition);
        send_counter = 0;
    }
    send_counter++;
    return (result);
}

/* 1 is error , 0 is ok */
uint32_t line_parameters_calculation(LINE_EDGE *EdgeArray, uint32_t EdgeCounter)
{
    uint32_t result = 0;
    uint32_t i = 0;

    int32_t line_shift = 0;
    int32_t line_width = 0;
    int32_t tmp = 0;
    if( EdgeCounter >= 2 )
    {
        for(i = 1; i < EdgeCounter; i++)
        {
            if( EdgeArray[i].Position > EdgeArray[i-1].Position)
            {
                line_width = line_width + (EdgeArray[i].Position - EdgeArray[i-1].Position);
            }
            else
            {
                result = 1;
            }
        }
        /* Make line width calculation. */
        if(result == 0)
        {
            line_width  = line_width/(EdgeCounter-1);
            /* Save current possition of line*/
            if(LineParameters.Valid == 0)
            {/* it is for start. */
                LineParameters.RightPosition = EdgeArray[1].Position;
                LineParameters.AbsolutPosition = EdgeArray[1].Position;
                LineParameters.Valid = 1;
            }
            line_shift = EdgeArray[1].Position - LineParameters.RightPosition;
            tmp = abs(line_shift);
            if( tmp > (line_width/2) )
            {
                if( line_shift < 0 )
                {
                    LineParameters.AbsolutPosition = LineParameters.AbsolutPosition + EdgeArray[2].Position - LineParameters.RightPosition;
                }
                if( line_shift > 0 )
                {
                    LineParameters.AbsolutPosition = LineParameters.AbsolutPosition - (LineParameters.RightPosition - EdgeArray[0].Position);
                }
            }
            else
            {
                LineParameters.AbsolutPosition = LineParameters.AbsolutPosition + EdgeArray[1].Position - LineParameters.RightPosition;
            }
            LineParameters.RightPosition = EdgeArray[1].Position;
            LineParameters.LineWidth = line_width;
        }
    }
    else
    {
        result = 1;
    }
    return (result);
}





